﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CSPCI1000;
using System.Threading;

namespace ContourSplit.Scaffold
{
	public class PCIBoard //:IDisposable
	{
		private Point loc;
		private double MaxV
		{
			get
			{
				return Config.config.VMax;
			}
		}
		private double MinV
		{
			get
			{
				return Config.config.VMin;
			}
		}
		private double Tacc
		{
			get
			{
				return Config.config.Tacc;
			}
		}
		private double mcperpx
		{
			get
			{
				return Config.config.MCPerPX;
			}
		}
		private double mcperdg
		{
			get
			{
				return Config.config.MCPerDG;
			}
		}
		private double mcPermm
		{
			get
			{
				return Config.config.MCPerMM;
			}
		}

		private const double epsilon = 1e-2;//avoid dividing zero
		public enum AXIS
		{
			X = 0, R, Y, Z
		};
		public PCIBoard()
		{
			uint x = PCIInvoke.w1000_initial();
			Console.WriteLine($"初始化...找到{x}个控制卡");
			loc = new Point();
			for (AXIS i = AXIS.X; i <= AXIS.Z; i++)
			{
				//脉冲模式1，脉冲下降沿有效
				PCIInvoke.w1000_set_pls_outmode((ushort)i, 1);
				//EL减速模式：0表示立即停止，1减速停止
				PCIInvoke.w1000_set_el((ushort)i, 0);
				//减速信号，0表示无效，1表示使能
				PCIInvoke.w1000_set_sd((ushort)i, 0);
				//ORG减速模式： 0 表示立即停止，1减速停止
				PCIInvoke.w1000_set_org((ushort)i, 0);
			}
		}
		public void SetPlsMode(ushort mode, int axis = -1)
		{
			if (axis < 0)
				for (AXIS i = AXIS.X; i <= AXIS.Z; i++)
				{
					PCIInvoke.w1000_set_pls_outmode((ushort)i, mode);
				}
			else
				PCIInvoke.w1000_set_pls_outmode((ushort)axis, mode);
		}
		public System.UInt32 MoveHome()
		{
			Console.WriteLine("回原点...");
			loc = new Point();
			SetPlsMode(0);
			for (AXIS i = AXIS.X; i <= AXIS.Z; i++)
			{
				var result = PCIInvoke.w1000_home_move((ushort)i, (int)Config.config.VMin, (int)Config.config.VMax, Config.config.Tacc);
				if (result != PCIInvoke.ERRCode.NoError)
				{
					return (uint)result;
				}
			}
			Console.WriteLine("Wait...");
			WaitForAllAxisMotionDone();
			Console.WriteLine("Done!");
			PCIInvoke.w1000_close();
			PCIInvoke.w1000_initial();
			return 0;
		}
		public System.UInt32 MoveXY(Point t)
		{
			double tan = (t.Y - loc.Y) / (t.X - loc.X + epsilon);
			double angle = Math.Atan(tan);
			if (t.X - loc.X < 0)
				angle += Math.PI;
			if (t.X - loc.X >= 0 && t.Y - loc.Y < 0)
				angle += Math.PI * 2;

			var x3 = TurnTo(angle / Math.PI * 180);
			Console.WriteLine($"转向: {angle / Math.PI * 180}度，{loc.ToString()}->{t.ToString()}, 相对位移(px):({t.X - loc.X},{t.Y - loc.Y}),Vx={(int)(MaxV * Math.Cos(angle))},Vy={(int)(MaxV * Math.Sin(angle))}");

			SetPlsMode(1, (int)AXIS.X);
			SetPlsMode(1, (int)AXIS.Y);
			var x1 = PCIInvoke.w1000_s_move((ushort)AXIS.X, (int)((t.X - loc.X) * mcperpx), (int)(MinV * Math.Cos(angle)), (int)(MaxV * Math.Cos(angle)), Tacc);
			var x2 = PCIInvoke.w1000_s_move((ushort)AXIS.Y, (int)((t.Y - loc.Y) * mcperpx), (int)(MinV * Math.Sin(angle)), (int)(MaxV * Math.Sin(angle)), Tacc);
			loc = t;
			if (x1 != PCIInvoke.ERRCode.NoError)
				return (uint)x1;
			if (x2 != PCIInvoke.ERRCode.NoError)
				return (uint)x2;
			WaitForMotionDone(AXIS.X);
			WaitForMotionDone(AXIS.Y);
			return 0;
		}

		/// <summary>
		/// 移动Z轴
		/// </summary>
		/// <param name="h">绝对位置。单位毫米</param>
		public System.UInt32 MoveZ(double h)
		{
			//SetPlsMode(0,(int)AXIS.Z);
			SetPlsMode(1, (int)AXIS.Z);
			Console.WriteLine($"Z绝对位置(脉冲)：{(int)(h*mcPermm)}");
			var x = PCIInvoke.w1000_sa_move((ushort)AXIS.Z, (int)(h * mcPermm), (int)MinV, (int)MaxV, Tacc);
			if (x != PCIInvoke.ERRCode.NoError)
				return (uint)x;
			WaitForMotionDone(AXIS.Z);
			return 0;
		}
		public System.UInt32 TurnTo(double d)
		{
			SetPlsMode(1, (int)AXIS.R);
			Console.WriteLine($"  转向{d}度，脉冲数：{(int)(d * mcperdg)}");
			var x = PCIInvoke.w1000_sa_move((ushort)AXIS.R, (int)(d * mcperdg), (int)MinV, (int)MaxV, Tacc);
			if (x != PCIInvoke.ERRCode.NoError)
				return (uint)x;
			WaitForMotionDone(AXIS.R);
			return 0;
		}
		/// <summary>
		/// 打开/关闭指定输出口
		/// </summary>
		/// <param name="port">输出口</param>
		/// <param name="data">true-打开;false-关闭</param>
		/// <returns></returns>
		public System.UInt32 Output(ushort port, bool data)
		{
			//这个函数0表示高，1表示低。
			var result = PCIInvoke.w1000_write_do(port, Convert.ToUInt16(!data));
			Console.WriteLine($"{port}号开关 {(data ? "打开" : "关闭")}，操作结果：{result}");
			return result;
		}
		public void WaitForMotionDone(AXIS axis)
		{
			for (;;)
			{
				Thread.Sleep(500);
				var e = PCIInvoke.w1000_motion_done((ushort)axis);
				Console.WriteLine($" 等待 {(ushort)axis} 号轴，结果： {(e != 0 ? "停止" : "运动")}");
				if (e != 0) return;
			}
		}
		public void WaitForAllAxisMotionDone()
		{
			for (AXIS a = AXIS.X; a <= AXIS.Z; a++)
			{
				WaitForMotionDone(a);
			}
		}
	}
}
